% p(y_n) = N(y_n | C*x_n, R)
% p(x_n | x_(n-1)) = N(x_n | A*x_(n-1), Q)
%
% [X, CovX, logp] = kalman_filter(x0, Covx0, Y, A, Q, C, R)

% Last modified 2011-10-19
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function [X, CovX, logp] = kalman_filter(x0, Covx0, Y, A, Q, C, R, method)

N = size(Y,2);
D = size(C,2);

X = zeros(D,N);
CovX = zeros(D,D,N);

if nargin < 8
  method = 1;
elseif method == 2
  R = inv(chol(R,'lower'));
  method = 2;
end

% Filtering step from the initial state
[X(:,1), CovX(:,:,1), logp] = kalman_filter_step(x0, ...
                                                 Covx0, ...
                                                 Y(:,1), ...
                                                 A, ...
                                                 Q, ...
                                                 C, ...
                                                 R, ...
                                                 method);

% Filter the signal
for n=2:N
  [X(:,n), CovX(:,:,n), l] = kalman_filter_step(X(:,n-1), ...
                                                CovX(:,:,n-1), ...
                                                Y(:,n), ...
                                                A, ...
                                                Q, ...
                                                C, ...
                                                R, ...
                                                method);
  logp = logp + l;
end